
#include <math.h>

class imu{

public:
    imu(){
        exInt = 0;
        eyInt = 0;
        ezInt = 0;
        q0 = 1;
        q1 = 0;
        q2 = 0;
        q3 = 0;
    }
    void process(float gx, float gy, float gz, float ax, float ay, float az,float dt) {
        float norm;
        //  float hx, hy, hz, bx, bz;
        float vx, vy, vz;// wx, wy, wz;
        float ex, ey, ez;
        
        // 先把这些用得到的值算好
        float q0q0 = q0*q0;
        float q0q1 = q0*q1;
        float q0q2 = q0*q2;
        //  float q0q3 = q0*q3;
        float q1q1 = q1*q1;
        //  float q1q2 = q1*q2;
        float q1q3 = q1*q3;
        float q2q2 = q2*q2;
        float q2q3 = q2*q3;
        float q3q3 = q3*q3;

        float halfT = dt/2;
        
        if(ax*ay*az==0)
            return;
            
        norm = invSqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
        ax = ax * norm;
        ay = ay * norm;
        az = az * norm;

        // estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁
        vx = 2*(q1q3 - q0q2);												//四元素中xyz的表示
        vy = 2*(q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3 ;

        // error is sum of cross product between reference direction of fields and direction measured by sensors
        ex = (ay*vz - az*vy) ;                           					 //向量外积在相减得到差分就是误差
        ey = (az*vx - ax*vz) ;
        ez = (ax*vy - ay*vx) ;

        exInt += ex;								  //对误差进行积分
        eyInt += ey;
        ezInt += ez;

        // adjusted gyroscope measurements
        gx = gx + MKp*ex + MKi*exInt;					   							//将误差PI后补偿到陀螺仪，即补偿零点漂移
        gy = gy + MKp*ey + MKi*eyInt;
        gz = gz + MKp*ez + MKi*ezInt;				   							//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

    // integrate quaternion rate and normalise						   //四元素的微分方程
        float pa = q1;
        float pb = q2;
        float pc = q3;
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = pa + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = pb + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = pc + (q0*gz + q1*gy - q2*gx)*halfT;

        // normalise quaternion
        norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 * norm;
        q1 = q1 * norm;
        q2 = q2 * norm;
        q3 = q3 * norm;
        
        printf("pitch:%f ;",asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3);; // pitch
        printf("roll:%f ;",atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3);; // pitch

    }
    float getQ0(void){
        return q0;
    }
    float getQ1(void){
        return q1;
    }
    float getQ2(void){
        return q2;
    }
     float getQ3(void){
        return q3;
    }
private:
    const float MKp = 2.0;           // proportional gain governs rate of convergence to accelerometer/magnetometer
    const float MKi = 1.0;          // integral gain governs rate of convergence of gyroscope biases // 0.005

    float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;   // scaled integral error
    float q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;  // quaternion elements representing the estimated orientation
 
    // Fast inverse square-root
    // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

    float invSqrt(float x) {
        float halfx = 0.5f * x;
        float y = x;
        long i = *(long*)&y;
        i = 0x5f3759df - (i>>1);
        y = *(float*)&i;
        y = y * (1.5f - (halfx * y * y));
        return y;
    }

};


